{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import csv\n",
    "import numpy as np\n",
    "import math"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0 : sensor_type : L\n",
      "1 : x_measured/rho_measured : 0.3122427\n",
      "2 : y_measured/phi_measured : 0.5803398\n",
      "3 : rhodot_measured : \n",
      "4 : timestamp : 1477010443000000 \n",
      "5 : x_groundtruth : 0.6\n",
      "6 : y_groundtruth : 0.6\n",
      "7 : vx_groundtruth : 5.199937\n",
      "8 : vy_groundtruth : 0\n",
      "9 : yaw_groundtruth : 0\n",
      "10 : yawrate_groundtruth : 0.006911322\n"
     ]
    }
   ],
   "source": [
    "lines = []\n",
    "with open(\"input.csv\") as csvf:\n",
    "    reader = csv.reader(csvf)\n",
    "    for line in reader:\n",
    "        lines.append(line)\n",
    "for i in range(len(lines[0])):\n",
    "    print(i,\":\",lines[0][i],\":\",lines[1][i])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initialize\n",
    "P = np.array([[1, 0, 0, 0],\n",
    "              [0, 1, 0, 0],\n",
    "              [0, 0, 1000, 0],\n",
    "              [0, 0, 0, 1000]], dtype=np.float)\n",
    "F = np.array([[1, 0, 1, 0],\n",
    "              [0, 1, 0, 1],\n",
    "              [0, 0, 1, 0],\n",
    "              [0, 0, 0, 1]], dtype=np.float)\n",
    "\n",
    "pre_time = int(lines[1][4])\n",
    "start = 1\n",
    "if lines[start][0] == \"L\":\n",
    "    px = float(lines[start][1])\n",
    "    py = float(lines[start][2])\n",
    "    vx = 0\n",
    "    vy = 0\n",
    "    X = np.array([[px],[py],[vx],[vy]], dtype=np.float)\n",
    "else :\n",
    "    px = math.cos(float(lines[start][2])) * float(lines[start][1])\n",
    "    py = math.sin(float(lines[start][2])) * float(lines[start][1])\n",
    "    vx = 0\n",
    "    vy = 0\n",
    "    X = np.array([[px],[py],[vx],[vy]], dtype=np.float)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from helper import *\n",
    "\n",
    "with open(\"output.csv\",\"w\") as csv_out: \n",
    "    out_writer = csv.writer(csv_out)\n",
    "    out_writer.writerow([\"sensor_type\",\"px\",\"py\",\"vx\",\"vy\",\"timestamp\",\"ture_x\",\"true_y\",\"error\"])\n",
    "\n",
    "    for line in lines[start+1:]:\n",
    "        now_time = int(line[4])\n",
    "        dt = (now_time - pre_time)/1000000\n",
    "        X_pred, P_pred = pred(X, P, dt)\n",
    "    \n",
    "        # update\n",
    "\n",
    "        if line[0] == \"L\":\n",
    "            Z = np.array([[float(line[1])],[float(line[2])]])\n",
    "            X, P = laser_update(X_pred, P_pred, Z)\n",
    "            error = abs(float(line[5])-X[0][0])+abs(float(line[6])-X[1][0])\n",
    "            out_writer.writerow([\"L\",X[0][0],X[1][0],X[2][0],X[3][0],now_time,line[5],line[6],error])\n",
    "            pre_time = now_time \n",
    "\n",
    "        else:\n",
    "            if float(line[2])<=math.pi:\n",
    "                Z = np.array([[float(line[1])], [float(line[2])], [float(line[3])]], dtype=np.float)\n",
    "                X, P = radar_update(X_pred, P_pred, Z)\n",
    "            else:\n",
    "                X, P = X_pred, P_pred\n",
    "            error = abs(float(line[5])-X[0][0])+abs(float(line[6])-X[1][0])\n",
    "            out_writer.writerow([\"R\",X[0][0],X[1][0],X[2][0],X[3][0],now_time,line[5],line[6],error])\n",
    "            pre_time = now_time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
